#include "simulation.h"

static
btDynamicsWorld* createDefaultWorld(void)
{
    btCollisionConfiguration * collision_config = new btDefaultCollisionConfiguration;
    //Stefan choice
    //btBroadphaseInterface *    broadphase       = new btAxisSweep3( btVector3(-1000,-1000,-1000), btVector3(1000,1000,1000) );
    btBroadphaseInterface *    broadphase       = new btDbvtBroadphase;
    btCollisionDispatcher *    dispatcher       = new btCollisionDispatcher( collision_config );
    btConstraintSolver *       solver           = new btSequentialImpulseConstraintSolver;

    return new btDiscreteDynamicsWorld( dispatcher, broadphase, solver, collision_config );
}

static
btRigidBody* buildBoxyBody(
        const btVector3& dimension,
        const btVector3& position = btVector3(btScalar(0.),btScalar(0.),btScalar(0.)),
        const btScalar   mass = btScalar(0.));

static
btRigidBody* buildBoxyBody(
        const btVector3& dimension,
        const btVector3& position,
        const btScalar   mass)
{
    btCollisionShape* collision_shape = new btBoxShape(dimension);

    btTransform       transform;
    transform.setIdentity();
    transform.setOrigin(position);

    //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
    btMotionState*    motionstate = new btDefaultMotionState(transform);

    btRigidBody::btRigidBodyConstructionInfo construction_information(mass,motionstate,collision_shape);

    return new btRigidBody(construction_information);
}

SimulationWorld::SimulationWorld(
        btScalar maximum_duration,
        btScalar width, btScalar height, btScalar depth,
        btScalar gravity_x, btScalar gravity_y, btScalar gravity_z)
    : m_world(createDefaultWorld())
    , m_population()
    , m_simulation_time(0)
    , m_maximum_simulation_duration(maximum_duration)
    , m_world_dimension(width, height, depth)
    , m_world_gravity(gravity_x, gravity_y, gravity_z)
{
    btRigidBody* ground_body = buildBoxyBody( m_world_dimension );
}

SimulationWorld::~SimulationWorld(void)
{
    delete m_world;
}

bool SimulationWorld::updateWorld(btScalar time_delta)
{
    if( m_simulation_time < m_maximum_simulation_duration )
    {
        m_world->stepSimulation( time_delta );
        m_simulation_time += time_delta;

        return true;
    }
    else
    {
        return false;
    }
}

SimulationWorld::IndividualCollection SimulationWorld::population() const
{
    return m_population;
}

bool SimulationWorld::add(Individual *individual)
{
    return m_population.insert( individual ).second;
}

bool SimulationWorld::remove(Individual *individual)
{
    return m_population.erase( individual ) == 1;
}


